#include "lidar_localization/models/graph_optimizer/g2o/edge/edge_odom.hpp"
#include "lidar_localization/models/graph_optimizer/preintegration/preintegration.hpp"
namespace g2o
{
    EdgeOdom::EdgeOdom() : BaseMultiEdge<6, std::pair<Eigen::Vector3d, Eigen::Quaterniond>>()
    {
        resize(4);
    }
    void EdgeOdom::computeError()
    {
        Eigen::Vector3d Pi = (dynamic_cast<VertexVec *>(_vertices[0]))->estimate();
        Eigen::Quaterniond Qi = (dynamic_cast<VertexQ *>(_vertices[1]))->estimate();
        Eigen::Vector3d Pj = (dynamic_cast<VertexVec *>(_vertices[2]))->estimate();
        Eigen::Quaterniond Qj = (dynamic_cast<VertexQ *>(_vertices[3]))->estimate();
        
        _error.head(3) = Qi.inverse() * (Pj - Pi) - _measurement.first;
        _error.tail(3) = 2 * (_measurement.second.inverse() * (Qi.inverse() * Qj)).vec();
    }
    void EdgeOdom::linearizeOplus()
    {
        Eigen::Vector3d Pi = (dynamic_cast<VertexVec *>(_vertices[0]))->estimate();
        Eigen::Quaterniond Qi = (dynamic_cast<VertexQ *>(_vertices[1]))->estimate();
        Eigen::Vector3d Pj = (dynamic_cast<VertexVec *>(_vertices[2]))->estimate();
        Eigen::Quaterniond Qj = (dynamic_cast<VertexQ *>(_vertices[3]))->estimate();

        Eigen::Matrix<double, 6, 12> jacs;
        jacs.setZero();
        jacs.block<3, 3>(0, 0) = -Qi.inverse().toRotationMatrix();
        jacs.block<3, 3>(0, 3) = lidar_localization::skewSymmetric(Qi.inverse() * (Pj - Pi));
        jacs.block<3, 3>(0, 6) = Qi.inverse().toRotationMatrix();

        jacs.block<3, 3>(3, 3) = -(lidar_localization::Qleft(Qj.inverse() * Qi) * lidar_localization::Qright(_measurement.second)).bottomRightCorner<3, 3>();
        jacs.block<3, 3>(3, 9) = (lidar_localization::Qleft(_measurement.second.inverse() * Qi.inverse() * Qj)).bottomRightCorner<3, 3>();

        for (unsigned int i = 0; i < _vertices.size(); i++)
        {
            _jacobianOplus[i] = jacs.block<6, 3>(0, 3 * i);
        }
    }
} // namespace g2o